#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <iostream>
#include <cmath>

class SimpleOdometryDemo
{
private:
    ros::NodeHandle nh_;
    ros::Subscriber odom_sub_;
    ros::Publisher cmd_vel_pub_;
    
    nav_msgs::Odometry start_odom_;
    nav_msgs::Odometry current_odom_;
    bool has_start_odom_;
    bool is_moving_;
    double target_distance_;

public:
    SimpleOdometryDemo() : has_start_odom_(false), is_moving_(false), target_distance_(0.0)
    {
        odom_sub_ = nh_.subscribe("/odom", 10, &SimpleOdometryDemo::odomCallback, this);
        cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
        
        ROS_INFO("简单里程计演示程序已启动");
        ROS_INFO("使用方法:");
        ROS_INFO("1. 输入目标距离（米）");
        ROS_INFO("2. 程序将自动控制小车移动");
        ROS_INFO("3. 实时显示移动距离");
    }

    void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
    {
        current_odom_ = *msg;
        
        if (!has_start_odom_)
        {
            start_odom_ = current_odom_;
            has_start_odom_ = true;
            ROS_INFO("记录起始位置: (%.3f, %.3f)", 
                     start_odom_.pose.pose.position.x, 
                     start_odom_.pose.pose.position.y);
        }
        
        // 计算当前移动距离
        double dx = current_odom_.pose.pose.position.x - start_odom_.pose.pose.position.x;
        double dy = current_odom_.pose.pose.position.y - start_odom_.pose.pose.position.y;
        double current_distance = sqrt(dx*dx + dy*dy);
        
        // 实时显示距离信息
        static int count = 0;
        if (++count % 10 == 0) // 每10次显示一次
        {
            ROS_INFO("当前移动距离: %.3f 米", current_distance);
            
            if (is_moving_ && current_distance >= target_distance_)
            {
                // 停止移动
                geometry_msgs::Twist stop_cmd;
                stop_cmd.linear.x = 0.0;
                cmd_vel_pub_.publish(stop_cmd);
                
                ROS_INFO("到达目标距离: %.3f 米", current_distance);
                ROS_INFO("实际移动距离: %.3f 米", current_distance);
                ROS_INFO("目标距离: %.3f 米", target_distance_);
                ROS_INFO("误差: %.3f 米", current_distance - target_distance_);
                
                is_moving_ = false;
            }
        }
    }

    void run()
    {
        ros::Rate rate(10);
        
        while (ros::ok())
        {
            if (!is_moving_)
            {
                std::cout << "\n请输入目标移动距离（米，输入0退出）: ";
                std::cin >> target_distance_;
                
                if (target_distance_ <= 0)
                {
                    ROS_INFO("程序退出");
                    break;
                }
                
                // 重置起始位置
                start_odom_ = current_odom_;
                has_start_odom_ = true;
                
                // 开始移动
                geometry_msgs::Twist move_cmd;
                move_cmd.linear.x = 0.2; // 0.2 m/s
                cmd_vel_pub_.publish(move_cmd);
                
                is_moving_ = true;
                ROS_INFO("开始移动 %.3f 米", target_distance_);
            }
            
            ros::spinOnce();
            rate.sleep();
        }
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "simple_odometry_demo");
    
    SimpleOdometryDemo demo;
    demo.run();
    
    return 0;
}
